%%%% Host Program for model
%%%% JSS 7/9/08
%%%% JSS 9/25/08


%% LOAD AND CHECK MODELS:
disp('loading models and checking xPC targets')
% virtual environment = model 1
model = 'model_protocol_healthy';
disp(['Model  = ', model])
% create target objects and unload other models
tg = xpctarget.xpc('TargetPC1');

% check & load target 1
if (strcmp(get(tg, 'Connected'), 'Yes'))
    disp('Target connected.  Loading model...')
    tg.unload;
    load(tg, model);
else
    beep
    disp('ERROR!  Target not connected!')
    return
end


% check target 
if (strcmp(get(tg, 'Application'), model))
    disp('xPC Virtual Environment Controller Load Complete.')
else
    beep
    disp('ERROR!  Model not loaded!')
    return
end

%% GET PARAMETER IDS:
pid = getparamid(tg, 'PID Controller/P Gain', 'Gain');
iid = getparamid(tg, 'PID Controller/I Gain', 'Gain');
did = getparamid(tg, 'PID Controller/D Gain', 'Gain');
main = getparamid(tg,'PID Controller/Main Gain', 'Gain');
ratio = getparamid(tg,'Gear Ratio', 'Value');
torque = getparamid(tg, 'Torque Step', 'Value');
bias = getparamid(tg, 'Output Voltage/PC104 Voltage Bias', 'Value');
 voltage = getparamid(tg, 'Output Voltage/Voltage', 'Value'); %for constant
practice = getparamid(tg, 'Practice', 'Value');



heel_strike_thresh_R = getparamid(tg,'Torque Step ON/R_incr_and_low/heel_strike_thresh','Value');
force_init_gain_RH = getparamid(tg,'Torque Step ON/R_incr_and_low/force_comp_init_H','Value');
force_init_gain_RL = getparamid(tg,'Torque Step ON/R_incr_and_low/force_comp_init_L','Value');
heel_strike_thresh_RP = getparamid(tg,'Torque Step ON/R_incr_and_low/pos_heel_strike_thresh','Value');

heel_strike_thresh_L = getparamid(tg,'Torque Step ON/L_incr_and_low/heel_strike_thresh','Value');
force_init_gain_LHt = getparamid(tg,'Torque Step ON/L_incr_and_low/force_comp_init_H','Value');
force_init_gain_LLt = getparamid(tg,'Torque Step ON/L_incr_and_low/force_comp_init_L','Value');
heel_strike_thresh_LP = getparamid(tg,'Torque Step ON/L_incr_and_low/pos_heel_strike_thresh','Value');

left_tread_heel_thresh = getparamid(tg,'Torque Step ON/L_pos_incr_and_R_low/left_tread_heel_thresh','Value');
force_init_gain_LTSH = getparamid(tg,'Torque Step ON/L_pos_incr_and_R_low/force_comp_init_H','Value');

left_tread_toe_off = getparamid(tg,'Torque Step ON/L_pos_decr_and_R_low/left_tread_toe_off','Value');
force_init_gain_LTEL = getparamid(tg,'Torque Step ON/L_pos_decr_and_R_low/force_comp_init_L','Value');
force_init_gain_LTEH = getparamid(tg,'Torque Step ON/L_pos_decr_and_R_low/force_comp_init_H','Value');

right_tread_toe_off = getparamid(tg,'Torque Step ON/R_pos_decr_and_L_low/right_tread_toe_off','Value');
force_init_gain_RTEL = getparamid(tg,'Torque Step ON/R_pos_decr_and_L_low/force_comp_init_L','Value');
force_init_gain_RTEH = getparamid(tg,'Torque Step ON/R_pos_decr_and_L_low/force_comp_init_H','Value');

right_tread_heel_thresh = getparamid(tg,'Torque Step ON/R_pos_incr_and_L_low/right_tread_heel_thresh','Value');
force_init_gain_RTSH = getparamid(tg,'Torque Step ON/R_pos_incr_and_L_low/force_comp_init_H','Value');

mid_decr_thresh_R = getparamid(tg,'Torque Step ON/RP_LP_decr_same/decr_thresh1','Value');
mid_decr_thresh_L = getparamid(tg,'Torque Step ON/RP_LP_decr_same/decr_thresh2','Value');


catch_trials = getparamid(tg,'Torque Step ON/catch_trials','Value');
record_trials = getparamid(tg,'Torque Step ON/record_trials','Value');
fam_end = getparamid(tg,'Torque Step ON/fam_end','Value');
wash_start = getparamid(tg,'Torque Step ON/wash_start','Value');
record_width = getparamid(tg,'Torque Step ON/record_width','Value');
max_step = getparamid(tg, 'max_step', 'Value');

refrac = getparamid(tg,'Torque Step ON/refractory_period','Value');
refractory_final = getparamid(tg,'Torque Step ON/refractory_final','Value');
force_on_time = getparamid(tg, 'force_on_time', 'Value');
renewal_freq = getparamid(tg,'Torque Step ON/renewal_frequency','Value');

bef_TO = getparamid(tg,'Torque Step ON/bef_TO','Value');
timing_type = getparamid(tg,'Torque Step ON/timing_type','Value');
% voltage to motor
% voltageid = getparamid(tg, 'High voltage', 'Value');
%% Signal IDs
clockid = getsignalid(tg,'Clock');


%% Starting Target and entering initial params
start(tg);
set(tg,'StopTime',1800);

%% PID Gains (probably shouldn't touch)
a = getparam(tg, pid);
setparam(tg, pid, .21);  %Load
setparam(tg, iid,0.0000);
setparam(tg, did, 0.0012);
setparam(tg,main,2.4);
setparam(tg,voltage,5.0);

setparam(tg,practice,0); % Enter 0 for torque trials, enter 1 for practice without brace

%% Enter height, weight, speed here:
walking_speed = 2.0 * 1/3.6; % m/s
height = 67 * 0.0254; % m
weight = 160 * 0.453; % kg
% torqueset_initial = 0.1460*walking_speed - 0.0373; % Nm/kg/m
% torqueset = torqueset_initial*height*weight;
torqueset = 40;


%% Entering Threshold parameters

setparam(tg,heel_strike_thresh_R,0.001);
setparam(tg,force_init_gain_RH,0.3);
setparam(tg,force_init_gain_RL,0.04);
setparam(tg,heel_strike_thresh_RP,0.0002);


setparam(tg,heel_strike_thresh_L,0.001);
setparam(tg,force_init_gain_LHt,0.3);
setparam(tg,force_init_gain_LLt,0.04);
setparam(tg,heel_strike_thresh_LP,0.0002);

setparam(tg,left_tread_heel_thresh,0.0003);
setparam(tg,force_init_gain_LTSH,0.001); 

setparam(tg,left_tread_toe_off,-0.0002);
setparam(tg,force_init_gain_LTEL,0.04);
setparam(tg,force_init_gain_LTEH,0.7);

setparam(tg,right_tread_toe_off,-0.0002); 
setparam(tg,force_init_gain_RTEL,0.04);
setparam(tg,force_init_gain_RTEH,0.7);

setparam(tg,right_tread_heel_thresh,0.0003); 
setparam(tg,force_init_gain_RTSH,0.001); 

setparam(tg,mid_decr_thresh_R,-0.0002);
setparam(tg,mid_decr_thresh_L,-0.0002);

end_fam = 50; training = 560; wash = 150;
setparam(tg,renewal_freq,1);
setparam(tg,record_trials,[40  105  151  185  261  320  373 440 481 550]+end_fam);
setparam(tg,catch_trials,[70  220  400  500]+end_fam);
setparam(tg,fam_end,end_fam);
setparam(tg,wash_start,end_fam+training);
setparam(tg,record_width,5);
setparam(tg,max_step,end_fam+training+wash);  %680

setparam(tg,bef_TO,0.20);
setparam(tg,timing_type,0); % 1 for CHS timing, 0 for timed
setparam(tg,refrac,0.3); 
setparam(tg,refractory_final,1);

%% Other parameters
% gear_ratio = 0.74;
gear_ratio = 0.58;
ger = gear_ratio*1000;
setparam(tg,torque,torqueset);
setparam(tg,bias,-0.0); %bias should be set to -0.025
setparam(tg,ratio,gear_ratio); 
setparam(tg,force_on_time,20);  % time for forces to come on 
%% DATA ACQUISITION LOOP:
i = 0;
while strcmp(tg.Status, 'running')
     i = i+1;
     [clock(i)] = getsignal(tg,clockid);
end
stop(tg);


%% Data collection

% fsys = xpctarget.fs;
% h = fsys.fopen('data.dat');
% data = fsys.fread(h);
% fsys.fclose(h);
% 
% new_data = readxpcfile(data);
% ndata = new_data.data;

time = tg.timelog;
data = tg.outputlog;
new_data = [time,data];
% new_data_2 = data;
%      eval(['save Step_test_debug_straight_no_pulley',num2str(torqueset),'.dat new_data -ASCII'])
%    eval(['save Torque_test_s',num2str(torqueset),'.dat new_data -ASCII'])
%  eval(['save Transparency_test_gr0',num2str(gear_ratio*1000),'.dat new_data -ASCII'])
%   new_data = load('forces_eddie_7_1_08_torque_60.dat','-ASCII');
    %eval(['save forces_jim_2km.dat new_data -ASCII'])
%% Analysis
time = new_data(:,1);
encoder = new_data(:,2);
des_pos = new_data(:,3);
cmd_voltage = new_data(:,4);
rotator = new_data(:,5);
activation =new_data(:,6);
R = new_data(:,7);
L = new_data(:,8);
RP = new_data(:,9);
LP =new_data(:,10);
RF = new_data(:,11);
RB =new_data(:,12);


link = des_pos;
knee = 2*des_pos;

commanded_torque = 6.5977*cmd_voltage;
actual_torque = (rotator-link)*0.368; %from best fit line to exp. data % ~30 Nm in 90 deg

close all;

figure; 
hold on;
plot(time,R,'r')
plot(time,L,'c')
plot(time,RP,'m')
plot(time,LP,'b')
plot(time,RF,'k','LineWidth',2)
plot(time,RB,'y')

% plot(time,cmd_voltage,'k')
%  plot(time,activation,'k')
%    plot(time,actual_torque/max(actual_torque),'g')





 

 
 